
#include <yaml_test/yaml_function.h>
//YAML::const_iterator find_name_return_it(std::string fin, std::string name_, geometry_msgs::Pose &pose);
//可以自定义模板的，但是感觉有点复杂，暂时先用着
namespace YAML_FUNCTION
{
    YAML::const_iterator Get_Specified_Address_Pose(std::string yaml_add, std::string address_name_, geometry_msgs::Pose &pose)
    {
        int yaml_size = 0;
        YAML::Node yamlConfig = YAML::LoadFile(yaml_add);
        YAML::const_iterator it = yamlConfig.begin();
        yaml_size = yamlConfig.size(); //获取yalm的size
        for(int i = 1;i<=yaml_size;i++)
        {
        
            if(it->first.as<std::string>() == address_name_)
            {
                std::cout << "had found:"  << address_name_<< std::endl;

                YAML::const_iterator yaml_it=  yamlConfig[address_name_].begin();
                ++yaml_it;
                for (i = 0; i<(yamlConfig[address_name_].size()-1); i++)
                {
                    ///能否用swtich + enum 取代if-else-if？
                    if(yaml_it->first.as<std::string>() == "pose.x")
                    {
                        pose.position.x =  yaml_it->second.as<float>();
                    }
                    else if(yaml_it->first.as<std::string>() == "pose.y")
                    {
                        pose.position.y =  yaml_it->second.as<float>();   
                    }
                    else if(yaml_it->first.as<std::string>() == "pose.z")
                    {
                        pose.position.z =  yaml_it->second.as<float>();   
                    }
                    else if(yaml_it->first.as<std::string>() == "orientation.w")
                    {
                        pose.orientation.w =  yaml_it->second.as<float>();   
                    }
                    else if(yaml_it->first.as<std::string>() == "orientation.x")
                    {
                        pose.orientation.x =  yaml_it->second.as<float>();   
                    }
                    else if(yaml_it->first.as<std::string>() == "orientation.y")
                    {
                        pose.orientation.y =  yaml_it->second.as<float>();   
                    }
                    else if(yaml_it->first.as<std::string>() == "orientation.z")
                    {
                        pose.orientation.z =  yaml_it->second.as<float>();   
                    }
                    ++yaml_it;
                }
                std::ofstream file;
                file.open(yaml_add);
                file.flush();
                file << yamlConfig;
                file.close();
                return yamlConfig[address_name_].begin();
            }
            it++;
        }
        std::cout << "can not found:"  << address_name_<< std::endl;
        std::ofstream file;
        file.open(yaml_add);
        file.flush();
        file << yamlConfig;
        file.close();
        exit(-1);
    }
}